Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Impementation of pid_tuning in a SCARA robot

Initial considerations

In order to use the pid_tining package, you need to be aware of two things, the list of supported ros_controllers and the task trajectories. Here you have a list of compatible ros_controllers:

• velocity_controllers
  • joint_position_controller
• effort_controllers
  • joint_position_controller
  • joint_velocity_controller

In this tutorial, the SCARA robot has a effort_controllers/joint_position_controller at each joint. Regarding the task trajectories, which are the trajectories that define the behavior of the robot, you must define each one of them. This trajectories are the cartesian space trajectory (M) and the joint space trajectory (J). In this case, M defines the desired trajectory for the end effector and J defines the desired angles or tralations for all joint at each time step, both J and M are discrete trajectories with the same number of elements, as you can see en the Figure:

Typically J can be obtained applaying the Inverse Kinematics with respect to M trajectory. The pid_tuning package requires that this trajectories be specified in CSV files, where for M CSV file, each column represents the X, Y and Z coordinates, and it has as many rows as number of elements. Similary for the J CSV file, each column is the desired desplacement (angular or traslational) for a specific joint, so in this file the number of columns is iqual to the number of robot controlled joints, and it has as many rows as the number of trajectory elements.

Cloning the SCARA robot description package

You need to clone the SCARA robot description package for run the dynamic simulation in Gazebo, to do this, follow the instructions below:

1. In the terminal, move to the catkin workspace in which you want to have your robot description package.

   cd ~/<workspace_name>/src

2. Clone the github scara robot package repository

   git clone https://github.com/We-R22/scara_robot_description.git

3. Return to your workspace

   cd ~/<workspace_name>

4. Compile the workspace

   catkin_make

5. Add to the bash

   source devel/setup.bash

Now you have to check if everything is ok. To launch the scara robot simulation, in the same terminal run:

• roslaunch scara_robot_description scara_main.launch

If all goes well, you should be able to see the dynamic simulation of the scara robot in Gazebo as shown in the Figure below:

To end the simulation, press ctrl+c in the terminal.

Installation of the pid_tuning package

Once you have the scara robot simulation ready, it's time to install the pid tuning package:

1. In the terminal, move to the catkin workspace in which you want to have your robot description package

   cd ~/<workspace_name>/src

2. Clone the github scara robot package repository

   git clone https://github.com/We-R22/pid_tuning.git

3. Return to your workspace

   cd ~/<workspace_name>

4. Compile the workspace

   catkin_make

5. Add to the bash

   source devel/setup.bash

Creating the main tuning node

At this point you have the robot description package to simulate the scara robot and the pid_tuning package to tune its controllers. The next step is to create the main tuning node that will be in charge of linking both packages and developing the tuning process.

Before creating the main tuning node, you need to consider the main goal of this approach, which is the minimization of the position errors during the task in both spaces: e^J and e^M . In joint space the position error e^J is the difference between the desired joint trajectories and the current joint positions. In Cartesian space the position error e^M is the difference between the desired Cartesian trajectory and the current end-effector position.

To do this, the main node starts by generating a random population of N individuals, each individual is composed by an 1*m array, where m = 3*A because one controller has three PID gains (kp, ki and kd) and being A the number of controlled joints, thus the population is defined by a m*N matrix. The node will be testing sequentially each set of PID gains taken from the population (corresponding to an individual of the population). So, it first update the PID gains values, after that, sends the joint trajectories to the controllers commands, then the robot try to follow them, while at the same time the node is calculating the e^J and e^M errors based on the data acquire from the controllers current states and odometry data measured at end-effector link. Considering the e^J and e^M errors, only the best individuals will pass to the next generation, to continuing the searching process until reach a certain stop criteria.

When finish, the node will return the best set of PID gains founded. The pid_tuning package provides you with, at the moment, with two bio-inspired algorithms to carry out the searching process: Differential Evolution and Harmony Search, so you need to choose one of them.

Having said that, in order to be able of start the tuning process, the main node needs to know:

• the system paths for both csv files, which contains the joint trajectories and the Cartesian trajectory
• the controller command topic paths to sending these trajectories
• the controller state topics paths to know the errors in joint space
• the controller pid topics paths to dynamically reconfigure the PID gains based on the each individual of the population
• the PID gain boundaries to generate the random population

Sounds like a lot of things, right? but don't worry, the pid_tuning package makes it easy for you, you just need to create a json file with this information and the package will do the rest. Let's do it.

Establishing file system paths for CSV trajectories files and controllers topics

Go to your catkin_ws/src/scara_robot_description directory, and create a json file named paths.json inside the config directory with the following code:

{
    "trajectories": {
        "path": "/home/<user_name>/<workspace_name>/src/scara_robot_description/trajectories/j_trajectory.csv",
        "column_names": ["q1", "q2", "q3"],
        "floating_base_path": "/home/<user_name>/<workspace_name>/src/scara_robot_description/trajectories/m_trajectory.csv",
        "fb_column_names": ["X", "Y", "Z"]
    }
    ,
    "command_paths": [
        "/scara/base_link_link_01_position_controller/command",
        "/scara/link_01_link_02_position_controller/command",
        "/scara/link_02_link_03_position_controller/command" 
    ],
    "state_paths": [
        "/scara/base_link_link_01_position_controller/state",
        "/scara/link_01_link_02_position_controller/state",
        "/scara/link_02_link_03_position_controller/state"
    ],
    "client_paths": [
        "/scara/base_link_link_01_position_controller/pid",
        "/scara/link_01_link_02_position_controller/pid",
        "/scara/link_02_link_03_position_controller/pid"
    ],
    "pid_bounds": {
        "p":[
            0, 1000
        ],
        "i": [
            0, 20
        ], 
        "d": [
            0, 10
        ]
    }
}

As you can see, the path.json file is made up of five objects, which correspond to the five things that the node needs to know, as we said before.

• The trajectories object provides the node with the system paths to the files m_trajectory.csv and j_trajectory.cvs, which contain the trajectories for both Cartesian space and joint space. This object has a dictionary with four keys: path, column_names, floating_base_path and fb_column_names, which must be filled as:

  • path: paste the system file path of j_trajectory.csv, this file is required to minimize the joint space position error.

  • column_names: indicate with an array the names of the columns in j_trajectory.csv, it is important to note that these names must always be the same, that is, if your robot has ten controlled joints, the names of the columns must go from q1 to q10.

* floating_base_path: the term "floating base" refers to the robot link which we want to measure its position and linear velocity (odom data), in order to be able to calculate the error in Cartesian space when the robot develops the following trajectory task. Here you need to indicate the system file path to m_trajectory.cvs if you want to minimize the error in the Cartesian space, but in that case you robot description package must have a node that publish the odom data of the "floating base" (end-effector), in a topic called /odom. If you do not have a odom node, you just let this field with a empty array, but the searching process only will take into account the minimization of the joint space error. In this tutorial we will create a odom node.

• fb_column_names: indicate with an array the names of the columns in m_trajectory.csv, these column names must always be X, Y and Z, because their represents the coordinates of each element of the trajectory in the Cartesian space.

• command_paths: indicate with an array the pid command topic names associated to all the controllers. This is necessary to send the joint trajectories to the joints.

• state_paths: indicate with an array the pid state topic names associated to all the controllers. This is necessary to read the joint position errors to calculate the joint space position error e^J.

• client_paths: these paths are required to the main tuning node to be able of update de PID controller gains, you need to indicate using an array the pid topic names associated to all the controllers.

• pid_bounds: here you must indicate, by means of a dictionary, the upper and lower limits that you want to consider as the search space for each PID gain associated with all the controllers, that is, the main tuning node will use these range values to generate the random population. Note that, for example, the gain range p will be valid for all gains p, and similarly the other gains d and i for all controllers.

As I've said before, the approach implemented in the pid_tuning package considers the minimization of the e^J and e^M errors, but you can choose if you only wants to use the e^J error. However, considering both errors will refine the PID tuning searching process. In this tutorial we will be taking to account both errors, so you need to create the odom node which will be in charge of publish the position and linear velocity of the end-effector link.

Go to your catkin_ws/src/scara_robot_description directory and create a directory named scripts

cd ~/<workspace_name>/src/scara_robot_description
mkdir scripts

Move to scripts directory and create a python file named odom_node.py with the following code:

   1 #!/usr/bin/env python3
   2 
   3 import rospy
   4 from nav_msgs.msg import Odometry
   5 from std_msgs.msg import Header
   6 from gazebo_msgs.srv import GetLinkState, GetLinkStateRequest
   7 
   8 rospy.init_node('odom_node')
   9 odom_pub = rospy.Publisher('/odom', Odometry, queue_size=10)
  10 
  11 rospy.wait_for_service('/gazebo/get_link_state')
  12 get_link_srv = rospy.ServiceProxy('/gazebo/get_link_state', GetLinkState)
  13 
  14 odom = Odometry()
  15 header = Header()
  16 
  17 header.frame_id = '/odom'
  18 link = GetLinkStateRequest()
  19 link.link_name = 'link_03'
  20 link.reference_frame = 'world'
  21 
  22 rate = rospy.Rate(10) # 10Hz
  23 while not rospy.is_shutdown():
  24     try:
  25         result = get_link_srv(link)
  26         odom.pose.pose = result.link_state.pose
  27         odom.twist.twist = result.link_state.twist
  28         header.stamp = rospy.Time.now()
  29         odom.header = header
  30         odom_pub.publish(odom)
  31         rate.sleep()
  32     except rospy.ROSTimeMovedBackwardsException:
  33         pass

roscd scara_descripton_package/scripts
chmod +x odom_node.py

This script is just an example of how to implement the odom node in your robot description package, feel free to create your own version of the odom node if you wish, but note that it is mandatory that the odom topic name be /odom and provides at least the pose information using an Odometry message type.

Basically, this code uses Gazebo's get_link_state service to get the position and linear velocity of a desired robot link, based on information provided by the simulation. The most relevant things to know about this code are:

  19 link.link_name = 'link_03'
  20 link.reference_frame = 'world'

here in link_name we are specifying the robot link name in which we want to measure the position and linear velocity, in this case, link_03 is the end-effector link as you can see in the urdf robot description. On the other hand, reference_frame indicates the link name reference frame with respect we will obtain the measurements, for this specific case, world correspond to the robot base frame. In case if you want to reuse this code in another robot, you only need to modify these two lines.

Selecting the bio-inpired algorithm solver

Now you need to decide which bio-inspired algorithm you want to use for the tuning process in the main tuning node. At the moment, the pid_tuning package provides you with two different algorithms: Differential Evolution and Harmony Search. So you have to choose between them.

Let's take a look at how to implement both...

The code for a Differential Evolution based solver node

Go to your catkin_ws/src/scara_robot_description directory, and create a python file named tuning_node_de.py inside the scripts directory with the following code:

   1 #!/usr/bin/env python3
   2 
   3 import rospy
   4 import sys
   5 from pid_tuning.evolutive_algorithms.dif_evolution import DifferentialEvolution
   6 from pid_tuning.settings.control_gazebo import ControlGazebo
   7 
   8 reset_control = ControlGazebo()
   9 
  10 A = 3
  11 m = 9
  12 N = 10 
  13 Gm = 10000
  14 F = 0.65
  15 C = 0.85
  16 hz = 25
  17 
  18 de = DifferentialEvolution(N, m, Gm, F, C, A, '/home/<user_name>/<workspace_name>/src/scara_robot_description/config/paths.json', epsilon_1=1, tm=28800)
  19 X = de.gen_population()
  20 
  21 def evol_loop():
  22     rospy.init_node("tuning_node")
  23     file = open("best_pid_values_DE.txt", 'w')
  24 
  25     reset_control.init_values()
  26     rate = rospy.Rate(hz)
  27     while not rospy.is_shutdown():
  28         de.evaluate(X, reset_control, rate)
  29         X_best = de.dif_evolution(X, reset_control, rate)
  30         file.write(str(X_best))
  31         file.close()
  32         break
  33 
  34 if __name__ == '__main__':
  35     try:
  36         evol_loop()
  37     except rospy.ROSInterruptException:
  38         pass

roscd scara_descripton_package/scripts
chmod +x tuning_node_de.py

The code explined

   3 import rospy
   4 import sys
   5 from pid_tuning.evolutive_algorithms.dif_evolution import DifferentialEvolution
   6 from pid_tuning.settings.control_gazebo import ControlGazebo
   7 
   8 reset_control = ControlGazebo()

In the above code, we are importing the DifferentialEvolution class that has a ready to use implementation of the Differential Evolution algorithm and ControlGazebo class which we are going to use to control the simulation by means of reset_control object.

  10 A = 3
  11 m = 9
  12 N = 10 
  13 Gm = 10000
  14 F = 0.65
  15 C = 0.85
  16 hz = 25

Population parameters and algorithm-specific operators are set here, such as:

• A = the number of joints controlled, in this case we have three.
• m = the number of design variables, in this case we have nine because there are three gains in each PID controller.
• N = this parameter controls the number of individuals in the population, we have decided to use 10 individuals.
• Gm = the maximum number of generations

Differential Evolution in its DE/rand/1/bin variant has two specific operators that need to be adjusted:

• F = the mutant factor, you should set it in a range of [0-1].
• C = the Crossover factor, likewise it should be set to a range of [0-1].

hz is a very important parameter and it depends on the size of the trajectories, that is, on their number of elements. In this example, the trajectories that we are going to use have 99 elements. For example, if we have a trajectory with 100 elements and we want that the time duration of the trajectories will be approximately 4 seconds, then we need to publish each trajectory element every 0.040 s (T), which implies a publish frequency (F) of 25 Hz, considering the formula: F=1/T, in this case we have a trajectory size of 99, then it has a duration of 3.96 s.

A high publishing rate implies very slow search convergence, because it doesn't give controllers enough time to reach the desired positions.

  18 de = DifferentialEvolution(N, m, Gm, F, C, A, '/home/<user_name>/<workspace_name>/src/scara_robot_description/config/paths.json', epsilon_1=1, tm=28800)
  19 X = de.gen_population()

when you instantiate the DifferentialEvolution class, you need to send the parameters mentioned above (N, m, Gm, F, C, A) and also the system path file of the json file you created earlier.

The epsilon-1 parameter is used to set the maximum value of Cartesian error to consider that the constraint has been reached, that is, since it is extremely difficult to obtain a zero error in the trajectory following task, then we will use an epsilon value to "relax" the constraint, if the Cartesian error is less than this value, then we will consider that the constraint has been reached, so in this case an epsilon-1 value of 1 [rad] means that the average maximum error in each element of the trajectory (in Cartesian space) will be approximately 0.01 [rad], since the trajectory is composed of 99 items.

The tm parameter is related to one of the stop criteria implemented, which are three:

• when a period of time has been reached, which is indicated by tm in seconds
• when the maximum number of generations Gm has been reached
• when the absolute difference between the best and the worst individual of the population is less than a certain threshold value established internally by the algorithm, so you don't need to worry about it.

After that, we call the gen_population() method in order to generate the random initial population and store it in the X variable.

  21 def evol_loop():
  22     rospy.init_node("tuning_node")
  23     file = open("best_pid_values_DE.txt", 'w')
  24 
  25     reset_control.init_values()
  26     rate = rospy.Rate(hz)
  27     while not rospy.is_shutdown():
  28         de.evaluate(X, reset_control, rate)
  29         X_best = de.dif_evolution(X, reset_control, rate)
  30         file.write(str(X_best))
  31         file.close()
  32         break

In the evol_loop() function we are initializing the node, establishing the .txt file where after finish the tuning process the node will store the best individual found, we mean, an array containing the best PID gains for all the controllers, also it is necessary to call the init_values() method from the gazebo_control object (named reset_control), in order to reset all the involved parts, as the controllers, the gazebo simulation, etc. In the same way we need to set the node publish rate using the hz value defined before.

Next, in the same function is the main while loop of ROS, where we are first calling the evaluate() method of the Differential Evolution object (called de) created earlier, to evaluate the random population, i.e. for all individuals in the population, we are measuring the errors in both spaces in the robot task defined by the files m_trayectory and j_trayectories.

Based on this initial evaluated population, we begin the searching process by calling the evolution() method, passing it the newly evaluated population X, the reset_control object, and the publish node object. With this information, the search process will start, until one of the three stopping criteria is reached.

When the search process is finished, node stores the best PID gains found in a .txt file called best_pid_values_DE.

The code for a Harmony Search based solver node

Go to your catkin_ws/src/scara_robot_description directory, and create a python file named tuning_node_hs.py inside the scripts directory with the following code:

   1 #! /usr/bin/env python3
   2 
   3 import rospy
   4 import sys
   5 from pid_tuning.settings.control_gazebo import ControlGazebo
   6 from pid_tuning.evolutive_algorithms.evolutive_algorithms import HarmonySearch
   7 
   8 # objeto para reiniciar la simulacion
   9 reset_control = ControlGazebo()
  10 
  11 A = 3
  12 m = 9
  13 N = 10
  14 Gm = 10000
  15 r_accept = 0.7
  16 r_pa = 0.45
  17 hz = 25
  18 
  19 hs = HarmonySearch(N, m, Gm, A, r_accept, r_pa, '/home/<user_name>/<workspace_name>/src/scara_robot_description/config/paths.json', epsilon_1=1, tm = 28800)
  20 X = hs.gen_population()
  21 
  22 def evol_loop():
  23     rospy.init_node("tuning_node")
  24     file = open("best_pid_values_HS.txt", 'w')
  25 
  26     reset_control.init_values()
  27     rate = rospy.Rate(hz)
  28     while not rospy.is_shutdown():
  29         hs.evaluate(X, reset_control, rate)
  30         X_best = hs.harmony_search(X, reset_control, rate)  
  31         file.write(str(X_best))
  32         file.close()      
  33         break
  34 
  35 if __name__ == '__main__':
  36     try:
  37         evol_loop()
  38     except rospy.ROSInterruptException:
  39         pass

roscd scara_descripton_package/scripts
chmod +x tuning_node_hs.py

The code explined

As you can see, the structure of the script is the same with respect to the node based on Differential Evolution, only minor changes have been made, for example:

   6 from pid_tuning.evolutive_algorithms.evolutive_algorithms import HarmonySearch

Instead of importing the Differential Evolution class, we are now importing the HarmonySearh class

  15 r_accept = 0.7
  16 r_pa = 0.45

therefore, we need to set the Harmony Search specific operators, which are:

• r_accept: a random value from [0,1]
• r_pa: a random value from [0,1]

  30         X_best = hs.harmony_search(X, reset_control, rate)  

finally to start the searching process we are calling the harmony_searh() method instead of the dif_evolution() method

Starting the PID tuning process

And that's it, you are now ready to start the tuning process using either the Differential Evolution based node version or the Harmony Search based version. In this tutorial we will use the Differential Evolution node version as an example, but note that the process is always the same regardless of the algorithm.

Step 1: you need to launch the scara robot simulation:

1. In the terminal, move to the catkin workspace in which you have your scara robot description package

   cd ~/<workspace_name>

2. Add to the bash

   source devel/setup.bash

3. Launch the scara robot simulation

   roslaunch scara_robot_description scara_main.launch

Step 2: This step depends on whether you have decided to consider Cartesian space error minimization, in this tutorial we will consider it, so start the odom node:

1. In a new tab or terminal move to the catkin workspace in which you have your scara robot description package

   cd ~/<workspace_name>

2. Add to the bash

   source devel/setup.bash

3. Run the odom node

   rosrun scara_robot_description odom_node.py

Step 3. Finally, it's time to run the main tuning node based on Differential Evolution

1. In a new tab or terminal move to the catkin workspace in which you have your scara robot description package

   cd ~/<workspace_name>

2. Add to the bash

   source devel/setup.bash

3. run the tuning_node_de.py

   rosrun scara_robot_description tuning_node_de.py 

You should get something like:

Congratulations!! at this point, it only remains to wait for the search process to end, depending on which stop criterion is reached first.

Results and conclusions

When the search process is done you will have a .txt file named best_pid_values_HS, in your workspace directory, containing the best PID gains found as an array, you just need to copy these values to your control config .yaml file to set these values as the default PID gains in the robot description package.

In conclusion, you can apply the pid_tuning package in an easy way, as you just need to provide the paths that define the desired task of your robot and copy the required paths into a single json file and the package will do the rest internally, even you don't have need to worry about implementing the bio-inspired algorithms, they are out of the box.

This is the first step by step tutorial for the pid_tuning package, we will add more as soon as possible.